#include "optitopo.h"

using namespace std;
using namespace Eigen;

/**
 * @brief Construct a new optitopo::optitopo object
 * @param dim 粒子位置的维数
 * @param results_dim 待输出结果的维数
 */
optitopo::optitopo(int dim, int particle_num, int results_dim, int fov_reso, double _fov_max, double _fov_min)
{
    dim_ = dim;
    fov_resolution_ = fov_reso;
    fov_max_ = _fov_max;
    fov_min_ = _fov_min;
    particle_num_ = particle_num;
    curr_iter_ = 0;
    dt_ = new double[dim_];
    wstart_ = new double[dim_];
	wend_ = new double[dim_];
	C1_ = new double[dim_];
	C2_ = new double[dim_];
    upper_bound_ = new double[dim_];
	lower_bound_ = new double[dim_];
	range_interval_ = new double[dim_];
	particles_ = new Particle[particle_num_];
	w_ = new double[dim_];
	all_best_position_ = new double[dim_];

	results_dim_ = results_dim;

	if (results_dim_)
	{
		cam_results_ = new double[results_dim_];
        uav_results_ = new double[results_dim_];
        cout << "the result will be recorded" << endl;
    }

    env_gain_ = new double[fov_resolution_ + 1];
    // 初始化信息增益，默认开始时 四个相机均朝向正前方看
    for (int i = 0; i < fov_resolution_ + 1;i++){
        double tmp_alpha = fov_min_ + i * (fov_max_ - fov_min_) / fov_resolution_;
        // env_gain_[i] = exp(-tmp_alpha * tmp_alpha);
        env_gain_[i] = CalculateInfoGain(tmp_alpha, 0);
    }
}

// 析构函数
optitopo::~optitopo()
{
	if (particles_) { delete[]particles_; }
	if (upper_bound_) { delete[]upper_bound_; }
	if (lower_bound_) { delete[]lower_bound_; }
	if (range_interval_) { delete[]range_interval_; }
	if (dt_) { delete[]dt_; }
	if (wstart_) { delete[]wstart_; }
	if (wend_) { delete[]wend_; }
	if (w_) { delete[]w_; }
	if (C1_) { delete[]C1_; }
	if (C2_) { delete[]C2_; }
	if (all_best_position_) { delete[]all_best_position_; }
	if (cam_results_) { delete[]cam_results_; }
    if (uav_results_) { delete[]uav_results_; }
    if (env_gain_) { delete[]env_gain_; }
}



/**
 * @brief 利用求根公式求取拉普拉斯矩阵的次小特征值
 * @param Laplace 
 * @return double 
 */
double optitopo::SolveLambda2_Analytical(Matrix4d Laplace){
    double a = -Laplace(0, 1);
    double b = -Laplace(0, 2);
    double c = -Laplace(0, 3);
    double d = -Laplace(1, 2);
    double e = -Laplace(1, 3);
    double f = -Laplace(2, 3);
    complex<double> W(-0.5, sqrt(3) / 2);
    double A = -1;
    double B = 2 * a + 2 * b + 2 * c + 2 * d + 2 * e + 2 * f;
    double C = - 3 * a * b - 3 * a * c - 3 * a * d - 3 * b * c - 3 * a * e - 3 * b * d - 4 * a * f - 4 * b * e 
               - 4 * c * d - 3 * b * f - 3 * c * e - 3 * c * f - 3 * d * e - 3 * d * f - 3 * e * f;
    double D = 4 * a * b * c + 4 * a * b * e + 4 * a * c * d + 4 * a * b * f + 4 * b * c * d + 4 * a * c * f + 4 * a * d * e + 4 * b * c * e 
             + 4 * a * d * f + 4 * b * d * e + 4 * a * e * f + 4 * b * d * f + 4 * c * d * e + 4 * b * e * f + 4 * c * d * f + 4 * c * e * f;
    double P = (3 * A * C - B * B) / (3 * A * A);
    double Q = (27 * A * A * D - 9 * A * B * C + 2 * B * B * B) / (27 * A * A * A);
    complex<double> DELTA = pow(Q / 2, 2) + pow(P / 3, 3);  //DELTA 必须被定义为complex<double>，才能正常进行开方、开立方操作
    complex<double> DELTA_1 = pow(-Q / 2 + pow(DELTA, 1.0 / 2), 1.0 / 3); //必须用pow(x,1.0/5),才能完成开方操作
    complex<double> DELTA_2 = pow(-Q / 2 - pow(DELTA, 1.0 / 2), 1.0 / 3);
    complex<double> x1 = DELTA_1 + DELTA_2 - B / (3 * A);
    complex<double> x2 = W * DELTA_1 + W * W * DELTA_2 - B / (3 * A);
    complex<double> x3 = W * W * DELTA_1 + W * DELTA_2 - B / (3 * A);

    //由于实对称矩阵的特征值都是实数，故此可直接比较上述三个复数的模
    double norm_x1 = abs(x1);
    double norm_x2 = abs(x2);
    double norm_x3 = abs(x3);

    double temp = (norm_x1 < norm_x2) ? norm_x1 : norm_x2;
    temp = (temp < norm_x3) ? temp : norm_x3;
    return temp;
}


/**
 * @brief 利用Eigen库求取拉普拉斯矩阵的次小特征值
 * @param Laplace 
 * @return double 
 */
double optitopo::SolveLambda2_Eigen(Matrix4d Laplace){
    EigenSolver<MatrixXd> es(Laplace);
    MatrixXd eigenvalues = es.pseudoEigenvalueMatrix();
    // MatrixXd V = es.pseudoEigenvectors();
    int length = Laplace.rows();
    double eigen_array[length];
    for(int Li = 0; Li < length; Li++){
        eigen_array[Li] = eigenvalues(Li,Li);
    }
    // cout <<"eigen"<< eigen_array[3] << "  " << eigen_array[2] << "" << eigen_array[1] << endl;
    sort(eigen_array, eigen_array + length);

    return eigen_array[1]; //取次小特征值
}


/**
 * @brief 利用幂迭代求取拉普拉斯矩阵的次小特征值
 * @param Laplace 
 * @return double 
 */
double optitopo::SolveLambda2_PowerIteration(Matrix4d Laplace, int iter_times, double conver_thre){
    Vector4d eigenvector_1(0.5, 0.5, 0.5, 0.5); //对应于最小特征值的单位特征向量
    double k = 0.25;//由于对角线的迹小于8，故特征值之和小于8，改系数可选取4 
    Matrix4d tempL = Matrix4d::Identity() - k * Laplace - eigenvector_1 * eigenvector_1.transpose();
    Vector4d tempV(1, 2, 1, 1);

    // for (int i = 0; i < iter_times;i++){
    //     tempV = tempL * tempV.normalized();
    // }
    
    // int count = 0;
    double rate = 1;
    while(rate>conver_thre){
        double norm = tempV.norm();
        tempV = tempL * tempV.normalized();
        rate = abs(tempV.norm() / norm - 1);
        // count++;
    }
    // cout << "count " << count << endl;
    return (1 - tempV.norm()) / k;
}


/**
 * @brief 输出此时的权重
 * @param _theta_ij 从无人机i出发到无人机j的向量
 * @param _alpha_i 相机偏航角
 * @return double 
 */
double optitopo::GetWeight(double _theta_ij, double _alpha_i){
    //用于调整角度权重的系数，取决于相机的fov，此处fov=pi/2；该系数的调整需要保证即使
    //在相机视野外，仍有权重，不然一旦上一个目标脱离相机视野，相机的规划就会显得漫无目的
    double CoefficientFov = 0.2; 
    //有别于上一个系数，该系数着重考虑目标是否在相机运动范围内的权重
    double CoefficientTheta_exp = 6;
    double CoefficientTheta_thr = M_PI / 3; // TODO

    //目标在相机视野内的角度权重
    double Weight_fov = exp(-CoefficientFov * pow((_theta_ij - _alpha_i),2));
    //目标在相机运动范围内的角度权重
    double Weight_theta = 1/(pow((_theta_ij/CoefficientTheta_thr),CoefficientTheta_exp)+1);
    //总权重
    // Weight = Weight_fov * Weight_theta * Weight_dis;
    double Weight = Weight_fov * Weight_theta;
    return Weight;
}


/**
 * @brief 计算拉普拉斯矩阵的次小特征值
 * @param _mat_ij 两架无人机位置之间的相对夹角
 * @param _alpha  相机角度
 */
double optitopo::GetLambda2(Matrix<double,4,4> _mat_ij, Vector4d _alpha){
    int length = _alpha.size();
    MatrixXd Laplace = MatrixXd::Zero(length,length);

    //赋权拉普拉斯矩阵
    //选项1.这是能同时观测视野里所有无人机的
    for (int Li = 0; Li < length;Li++){
        for (int Lj = 0; Lj < length; Lj++)
        {
            if(Li==Lj){
                continue;
            }
            double SinglrWeight = GetWeight(_mat_ij(Li, Lj), _alpha(Li));
            Laplace(Li, Lj) = Laplace(Li, Lj) - SinglrWeight;
            Laplace(Lj, Li) = Laplace(Lj, Li) - SinglrWeight;
        }
    }
    //对角元素赋值部分
    VectorXd histogram_row = Laplace.rowwise().sum();
    for(int Li = 0; Li < length; Li++){
        Laplace(Li, Li) = -histogram_row(Li);
    }

    //选项2.只能观测一架无人机，且观测权重最大的一架
    // for (int Li = 0; Li < length;Li++){
    //     double max_weight = -1000.0;
    //     int max_index = 0;
    //     for (int Lj = 0; Lj < length; Lj++)
    //     {
    //         if(Li==Lj){
    //             continue;
    //         }
    //         else{
    //             double temp_weight = optitopo::GetWeight(theta[Li], theta[Lj], alpha[Li]);
    //             if(temp_weight>max_weight){
    //                 max_weight = temp_weight;
    //                 max_index = Lj;
    //             }
    //         }
    //     }
    //     // //有向图
    //     // Laplace(Li, Li) = max_weight;
    //     // Laplace(Li, max_index) = -max_weight;

    //     //无向图 非对角元素赋值部分
    //     Laplace(Li,Li)=0;
    //     Laplace(Li,max_index) = Laplace(Li,max_index) - max_weight;
    //     Laplace(max_index,Li) = Laplace(max_index,Li) - max_weight;
    //     this->SeeWho += 10 ^ (Li - 1) * max_index;
    // }
    // //无向图 对角元素赋值部分
    // VectorXd histogram_row = Laplace.rowwise().sum();
    // for(int Li = 0; Li < length; Li++){
    //     Laplace(Li, Li) = -histogram_row(Li);
    // }

    // cout << "Laplace" << Laplace << endl;
    //求迹
    // this->TraceofL = Laplace.trace();

    //求次小特征值
    double lambda2 = this->SolveLambda2_Analytical(Laplace); //取次小特征值
    // this->lambda2 = this->SolveLambda2_Eigen(Laplace); //取次小特征值
    return lambda2;
}

/**
 * @brief 计算该角度的信息增益 TODO
 * @param _angle 待计算信息增益的角度
 * @param _mid_angle 相机角 
 * @return double 
 */
double optitopo::CalculateInfoGain(double _angle, double _mid_angle){
    return exp(-(_angle - _mid_angle) * (_angle - _mid_angle) / env_gain_perish_cof_);
}


/**
 * @brief 获得这一时刻与上一时刻相比的环境信息增益
 * @param _alpha 这一时刻的相机转角
 * @return double 
 */
double optitopo::GetEnvInfoGain(Vector4d _alpha){
    double info_gain_total = 0;
    double angle[4] = {_alpha(0), _alpha(1), _alpha(2), _alpha(3)};
    sort(angle, angle + 4);
    double mid[3] = {(angle[0] + angle[1]) / 2.0, (angle[1] + angle[2]) / 2.0, (angle[2] + angle[3]) / 2.0};

    double k = 1 - Delta_t_ / env_gain_perish_time_;
    for (int i = 0; i < fov_resolution_ + 1; i++)
    {
        double tmp_alpha = fov_min_ + i * (fov_max_ - fov_min_) / fov_resolution_;
        double tmp_info_gain;
        if (tmp_alpha < mid[0]) {
            tmp_info_gain = CalculateInfoGain(tmp_alpha, angle[0]);
        }
        else if(tmp_alpha < mid[1]){
            tmp_info_gain = CalculateInfoGain(tmp_alpha, angle[1]);
        }
        else if(tmp_alpha < mid[2]){
            tmp_info_gain = CalculateInfoGain(tmp_alpha, angle[2]);
        }
        else{
            tmp_info_gain = CalculateInfoGain(tmp_alpha, angle[3]);
        }
        if (k > 0.0){
            double info_gain_reso = tmp_info_gain - k * env_gain_[i];
            if(info_gain_reso>0){
                info_gain_total += info_gain_reso;
            }
        }
        else{
            info_gain_total += tmp_info_gain;
        }
    }

    return info_gain_total;
}


/**
 * @brief 更新环境信息增益 
 */
void optitopo::UpdateEnvInfoGain(Vector4d _alpha){
    double info_gain_total = 0;
    double angle[4] = {_alpha(0), _alpha(1), _alpha(2), _alpha(3)};
    sort(angle, angle + 4);
    double mid[3] = {(angle[0] + angle[1]) / 2.0, (angle[1] + angle[2]) / 2.0, (angle[2] + angle[3]) / 2.0};

    double k = 1 - Delta_t_ / env_gain_perish_time_;
    Infoexe_ = 0.0;
    for (int i = 0; i < fov_resolution_ + 1; i++)
    {
        double tmp_alpha = fov_min_ + i * (fov_max_ - fov_min_) / fov_resolution_;
        double tmp_info_gain;
        if (tmp_alpha < mid[0]) {
            tmp_info_gain = CalculateInfoGain(tmp_alpha, angle[0]);
        }
        else if(tmp_alpha < mid[1]){
            tmp_info_gain = CalculateInfoGain(tmp_alpha, angle[1]);
        }
        else if(tmp_alpha < mid[2]){
            tmp_info_gain = CalculateInfoGain(tmp_alpha, angle[2]);
        }
        else{
            tmp_info_gain = CalculateInfoGain(tmp_alpha, angle[3]);
        }

        if (k > 0.0){
            if(tmp_info_gain> k * env_gain_[i]){
                env_gain_[i] = tmp_info_gain;
            }
            else{
                env_gain_[i] = k * env_gain_[i];
            }
        }
        else{
            env_gain_[i] = tmp_info_gain;
        }
        Infoexe_ += env_gain_[i];
    }
}


/**
 * @brief 计算势能 TODO：替换势函数
 * @param _k 比例系数
 * @return double 
 */
double optitopo::CalculatePotential(Vector2d _pos1, Vector2d _pos2, double _k)
{
    double tmp = (_pos1 - _pos2).norm();
    return -_k / (tmp * tmp);
}


/**
 * @brief 计算 环境势能 
 */
double optitopo::GetEnvPotential(Matrix<double, 2, 4> _formation,bool final_flag)
{
    double total_potential = 0;

    // Calculate Formation Potential
    for (int i = 0; i < 4;i++){
        for (int j = i; j < 4;j++){
            if(i==j){
                continue;
            }
            total_potential += 2 * CalculatePotential(_formation.block<2, 1>(0, i), _formation.block<2, 1>(0, j), formation_param_k_form_); // TODO
        }
    }

    if(final_flag){
        env_potential_form_ = total_potential;
    }

    // Calculate Obstacle Potential
    int obs_num = obs_means_.size();
    if (obs_num == 0){
        return total_potential;
    }

    for (int i = 0; i < obs_num; i++)
    {
        if(pi_vector_[i]<2){
            continue;
        }
        Vector2d pos_obs;
        pos_obs(0) = obs_means_[i](0);
        pos_obs(1) = obs_means_[i](1);

        for (int j = 0; j < 4; j++)
        {  
            Vector2d pos_uav = _formation.block<2, 1>(0, j);
            total_potential += CalculatePotential(pos_uav, pos_obs, log(pi_vector_[i]));  // TODO pi 的映射
            // total_potential += CalculatePotential(pos_uav, -pos_obs, log(pi_vector_[i])); // TODO 增加障碍物的中心对称势能
        }
    }
    return total_potential;
}


/**
 * @brief 根据队形参数 生成队形
 * @param _formation_param  0：scale on x; 1: scale on y; 2: shear about x; 3: shear about y;
 * @param _formation_new new formation
 */
void optitopo::UpdateFormation(Vector4d _formation_param, Matrix<double, 2, 4>& _formation_new)
{
    Vector2d vec_scale = _formation_param.head(2);
    Matrix2d mat_scale = vec_scale.asDiagonal();
    Matrix2d mat_shear = Matrix2d::Identity();
    mat_shear(0, 1) = tan(_formation_param(2));
    mat_shear(1, 0) = tan(_formation_param(3));

    Matrix2d mat_tran =  mat_shear * mat_scale;
    // cout << "mat_tran" << mat_tran << endl;
    for (int i = 0; i < 4; i++)
    {
        _formation_new.col(i) = mat_tran * formation_init_.col(i); 
    }
}


/**
 * @brief 根据队形，利用最小二乘法生成队形参数
 *                [a  a*tan(d)]
 * P_init{4x2} x  [           ]  = P_now{4x2} 
 *                [b*tan(c)  b]
 * @param _formation_init 
 * @param _formation_new 
 * @param _formation_param 
 */
bool optitopo::ConvertFormation2Param(Matrix<double, 2, 4> _formation_init, 
                                      Matrix<double, 2, 4> _formation_new, Vector4d &_formation_param)
{
    Matrix<double, 2, 2> tmp = (_formation_init * _formation_init.transpose()).inverse() * _formation_init * _formation_new.transpose();
    _formation_param(0) = tmp(0, 0);
    _formation_param(1) = tmp(1, 1);
    if (_formation_param(0) == 0 || _formation_param(1) == 0){
        cout << "Convert Formation to Param Failed !!!" << endl;
        return false;
    }
    _formation_param(2) = atan(tmp(1, 0) / tmp(1, 1));
    _formation_param(3) = atan(tmp(0, 1) / tmp(0, 0));
}


void optitopo::UpdateTheta(Matrix<double, 2, 4> _formation_new)
{
    for (int i = 0; i < 4;i++){
        for (int j = 0; j < 4;j++){
            if(i==j){
                theta_ij_(i, j) = 0;
            }
            else{
                theta_ij_(i, j) = atan2(_formation_new(1,j)-_formation_new(1,i), _formation_new(0,j)-_formation_new(0,i));
            }
        }
    }
}


void optitopo::GetBound(double _value_now, double _upbound, double _lowbound, double _step,
                        double &_upbound_out, double &_lowbound_out)
{
    double tmp_upbound = _value_now + _step / 2.0;
    double tmp_lowbound = _value_now - _step / 2.0;
    if (tmp_upbound < _upbound && tmp_lowbound > _lowbound)
    {
        _upbound_out = tmp_upbound;
        _lowbound_out = tmp_lowbound;
    }
    else if(tmp_upbound > _upbound || tmp_upbound == _upbound){
        _upbound_out = _upbound;
        _lowbound_out = _upbound - _step;
    }
    else{
        _lowbound_out = _lowbound;
        _upbound_out = _lowbound + _step;
    }
}


double optitopo::UpdateParamwithVel(double _param_last, double _param_new, double _param_vel)
{
    if(abs(_param_last-_param_new)<_param_vel){
        return _param_new;
    }
    if(_param_last<_param_new){
        return _param_last + _param_vel;
    }
    else{
        return _param_last - _param_vel;
    }
}

/**
 * @brief 利用粒子群进行搜索 最主要的函数！
 * @param _formation_param_last 上次的队形参数
 * @param _alpha 此刻的相机转角
 * @param _Delta_t 时间间隔
 * @return true 
 * @return false 
 */
bool optitopo::GetOutput_pso(Vector4d _formation_param_last, Vector4d _alpha, double _Delta_t){
    // 给类的内部成员赋值
    Delta_t_ = _Delta_t;
    alpha_in_ = _alpha;

    Matrix<double, 2, 4> _formation_last;
    UpdateFormation(_formation_param_last, _formation_last);
    // cout << "_formation_last" << _formation_last << endl;
    UpdateTheta(_formation_last);
    // cout << "theta_ij_" << theta_ij_ << endl;
    UpdateEnvInfoGain(_alpha);

    // 相机偏角搜索过程
    Iter_Type_ = 0;
    std::srand((unsigned int)time(0));
    for (int i = 0; i < dim_; i++)
    {
        double tmp_upbound;
        double tmp_lowbound;
        GetBound(alpha_in_(i), cam_yaw_bound_(0, i), cam_yaw_bound_(1, i), cam_yaw_step_, tmp_upbound, tmp_lowbound);

        //赋值给粒子
        upper_bound_[i] = tmp_upbound;
        lower_bound_[i] = tmp_lowbound;
        range_interval_[i] = tmp_upbound - tmp_lowbound;
    }

    InitialAllParticles();
    GetFinalResult();

    for(int i = 0; i < dim_; i++){
        alpha_out_(i) = all_best_position_[i];
    }

    Infogain_ = GetEnvInfoGain(alpha_out_);

    // 无人机队形搜索过程
    Iter_Type_ = 1;
    for (int i = 0; i < dim_; i++)
    {
        double tmp_upbound;
        double tmp_lowbound;
        GetBound(_formation_param_last(i), formation_param_bound_(0, i), formation_param_bound_(1, i),
                 formation_param_step_(i), tmp_upbound, tmp_lowbound);

        upper_bound_[i] = tmp_upbound;
        lower_bound_[i] = tmp_lowbound;
        range_interval_[i] = tmp_upbound - tmp_lowbound;
        // cout << "upper_bound_[i]" << upper_bound_[i] << endl;
        // cout << "lower_bound_[i]" << lower_bound_[i] << endl;
        // cout << "range_interval_" << range_interval_[i] << endl;
    }

    InitialAllParticles();
    GetFinalResult();

    for (int i = 0; i < dim_; i++)
    {
        formation_param_new_(i) = UpdateParamwithVel(_formation_param_last(i), all_best_position_[i], formation_param_vel_(i));
    }
    UpdateFormation(formation_param_new_, formation_new_);
    UpdateTheta(formation_new_);
    env_potential_total_ = GetEnvPotential(formation_new_,true);
    lambda2_ = GetLambda2(theta_ij_, alpha_out_);

    return true;
}


/**
 * @brief 初始化所有粒子
 */
void optitopo::InitialAllParticles()
{
    curr_iter_ = 0;
	// 初始化第一个粒子参数并设置最优值
	InitialParticle(0);
	all_best_fitness_ = particles_[0].best_fitness_;
	for (int j = 0; j < dim_; j++)
	{
		all_best_position_[j] = particles_[0].best_position_[j];
	}

	// 初始化其他粒子，并更新最优值
	for (int i = 1; i < particle_num_; i++)
	{
		InitialParticle(i);
#ifdef MAXIMIZE_FITNESS
		if (particles_[i].best_fitness_ > all_best_fitness_)
#else
		if (particles_[i].best_fitness_ < all_best_fitness_)
#endif
		{
			all_best_fitness_ = particles_[i].best_fitness_;
			for (int j = 0; j < dim_; j++)
			{
				all_best_position_[j] = particles_[i].best_position_[j];
			}

			// // 如果需要保存出一些结果 TODO
			// if (particles_[i].results_dim_)
			// {
			// 	for (int k = 0; k < results_dim_; k++)
			// 	{
			// 		results_[k] = particles_[i].results_[k];
			// 	}
			// }
			// else if (results_dim_)
			// {
			// 	std::cout << "WARNING: the dimension of your saved results for every particle\nis not match with the dimension you specified for PSO optimizer ant no result is saved!" << std::endl;
			// }
		}
	}
    if(Iter_Type_ ==0){
        cam_results_[curr_iter_] = all_best_fitness_; // TODO 记录结果可用
    }
    else{
        uav_results_[curr_iter_] = all_best_fitness_;
    }
}


/**
 * @brief 获取双精度随机数（默认精度为0.0001）
 * @param N 精度倒数
 * @return double 
 */
double optitopo::GetDoubleRand(int N)
{
	double temp = rand() % (N + 1) / (double)(N + 1);
	return temp;
}


/**
 * @brief 完成设定次数的迭代过程
 */
void optitopo::GetFinalResult(){
	for (int i = 0; i < max_iter_num_-1; i++)
	{
		UpdateAllParticles();
		// result[i] = psooptimizer.all_best_fitness_;
		// std::cout << "第" << i << "次迭代结果：";
		// std::cout << "x = " << psooptimizer.all_best_position_[0] << ", " << "y = " << psooptimizer.all_best_position_[1];
		// std::cout << ", fitness = " << result[i] << std::endl;
	}
}


/**
 * @brief 更新所有粒子参数
 */
void optitopo::UpdateAllParticles()
{
	GetInertialWeight();
	for (int i = 0; i < particle_num_; i++)
	{
		UpdateParticle(i);
#ifdef MAXIMIZE_FITNESS
		if (particles_[i].best_fitness_ > all_best_fitness_)
#else
		if (particles_[i].best_fitness_ < all_best_fitness_)
#endif
		{
			all_best_fitness_ = particles_[i].best_fitness_;
			for (int j = 0; j < dim_; j++)
			{
				all_best_position_[j] = particles_[i].best_position_[j];
			}
			
			// // 如果需要保存出一些参数
			// if (particles_[i].results_dim_ && results_dim_ == particles_[i].results_dim_)
			// {
			// 	for (int k = 0; k < results_dim_; k++)
			// 	{
			// 		results_[k] = particles_[i].results_[k];
			// 	}
			// }
			// else if (results_dim_)
			// {
			// 	std::cout << "WARNING: the dimension of your saved results for every particle\nis not match with the dimension you specified for PSO optimizer ant no result is saved!" << std::endl;
			// }
		}
	}
	curr_iter_++;
    // TODO 保存结果
    // if(Iter_Type_ ==0){
    //     cam_results_[curr_iter_] = all_best_fitness_;
    // }
    // else{
    //     uav_results_[curr_iter_] = all_best_fitness_;
    // }
}


/**
 * @brief 更新第i个粒子
 * @param i 
 */
void optitopo::UpdateParticle(int i)
{
	// 计算当前迭代的权重
	for (int j = 0; j < dim_; j++)
	{
		// 保存上一次迭代结果的position和velocity
		//double last_velocity = particles_[i].velocity_[j];
		double last_position = particles_[i].position_[j];

		particles_[i].velocity_[j] = w_[j] * particles_[i].velocity_[j] +
			C1_[j] * GetDoubleRand() * (particles_[i].best_position_[j] - particles_[i].position_[j]) +
			C2_[j] * GetDoubleRand() * (all_best_position_[j] - particles_[i].position_[j]);
		particles_[i].position_[j] += dt_[j] * particles_[i].velocity_[j];

		// 如果搜索区间有上下限限制
		if (upper_bound_ && lower_bound_)
		{
			if (particles_[i].position_[j] > upper_bound_[j])
			{
				double thre = GetDoubleRand(99);
				if (last_position == upper_bound_[j])
				{
					particles_[i].position_[j] = GetDoubleRand() * range_interval_[j] + lower_bound_[j];
				}
				else if (thre < 0.5)
				{
					particles_[i].position_[j] = upper_bound_[j] - (upper_bound_[j] - last_position) * GetDoubleRand();
				}
				else
				{
					particles_[i].position_[j] = upper_bound_[j];
				}		
			}
			if (particles_[i].position_[j] < lower_bound_[j])
			{
				double thre = GetDoubleRand(99);
				if (last_position == lower_bound_[j])
				{
					particles_[i].position_[j] = GetDoubleRand() * range_interval_[j] + lower_bound_[j];
				}
				else if (thre < 0.5)
				{
					particles_[i].position_[j] = lower_bound_[j] + (last_position - lower_bound_[j]) * GetDoubleRand();
				}
				else
				{
					particles_[i].position_[j] = lower_bound_[j];
				}
			}
		}
	}

	if(Iter_Type_==0){
        particles_[i].fitness_ = GetFitness_Camera(particles_[i]);
    }
    else{
        particles_[i].fitness_ = GetFitness_Uav(particles_[i]);
    }

#ifdef MAXIMIZE_FITNESS
	if (particles_[i].fitness_ > particles_[i].best_fitness_)
#else
	if (particles_[i].fitness_ < particles_[i].best_fitness_)
#endif
	{
		particles_[i].best_fitness_ = particles_[i].fitness_;
		for (int j = 0; j < dim_; j++)
		{
			particles_[i].best_position_[j] = particles_[i].position_[j];
		}
	}
}


/**
 * @brief 获取当前迭代的权重
 */
void optitopo::GetInertialWeight()
{
	double temp = curr_iter_ / (double)max_iter_num_;
	temp *= temp;
	for (int i = 0; i < dim_; i++)
	{
		w_[i] = wstart_[i] - (wstart_[i] - wend_[i]) * temp;
	}
}


/**
 * @brief 初始化第i个粒子参数
 * @param i 
 */
void optitopo::InitialParticle(int i)
{
	// 为每个粒子动态分配内存
	particles_[i].position_ = new double[dim_];
	particles_[i].velocity_ = new double[dim_];
	particles_[i].best_position_ = new double[dim_];

	//if (results_dim_)
	//{
	//	particles_[i].results_ = new double[results_dim_];
	//}

	// 初始化position/veloctiy值
	for (int j = 0; j < dim_; j++)
	{
		// if defines lower bound and upper bound
		if (range_interval_)
		{
			particles_[i].position_[j] = GetDoubleRand() * range_interval_[j] + lower_bound_[j];
			particles_[i].velocity_[j] = GetDoubleRand() * range_interval_[j];  // TODO :修改初始速度
			//std::cout << particles_[i].position_[j] << std::endl;
		}
		else
		{
			particles_[i].position_[j] = GetDoubleRand() * 2;
			particles_[i].velocity_[j] = GetDoubleRand() * 0.5;
		}
	}

	// 设置初始化最优适应度值
    if(Iter_Type_==0){
        particles_[i].fitness_ = GetFitness_Camera(particles_[i]);
    }
    else{
        particles_[i].fitness_ = GetFitness_Uav(particles_[i]);
    }

	for (int j = 0; j < dim_; j++)
	{
		particles_[i].best_position_[j] = particles_[i].position_[j];
	}
	particles_[i].best_fitness_ = particles_[i].fitness_;
}


/**
 * @brief 以相机转角为变量，计算优化函数
 */
double optitopo::GetFitness_Camera(Particle& particle){
    Vector4d alpha_tmp;
    for (int i = 0; i < dim_; i++)
    {
        alpha_tmp(i) = particle.position_[i];
    }

    // Calculate Swarm Connectivity
    double J_connect = GetLambda2(theta_ij_, alpha_tmp);//TODO

    // Calculate Environment Information Gain
    double J_env = GetEnvInfoGain(alpha_tmp);//TODO

    // cout << "J_env" << J_env << endl;
    // cout << "J_connect" << J_connect << endl;

    return k_con_ * J_connect + k_env_ * J_env;
    // return  J_env ;
}


/**
 * @brief 以无人机位置为变量，计算优化函数 
 */
double optitopo::GetFitness_Uav(Particle& particle){
    Vector4d formation_param;
    Matrix<double, 2, 4> formation_new;

    for (int i = 0; i < dim_; i++)
    {
        formation_param(i) = particle.position_[i];
    }
    // cout << "formation_param" << formation_param << endl;

    UpdateFormation(formation_param, formation_new);
    
    // Calculate Swarm Connectivity
    UpdateTheta(formation_new);
    double J_connect = GetLambda2(theta_ij_, alpha_out_);

    // Calculate Formation and Obstacle Potential
    double J_fit = GetEnvPotential(formation_new,false);

    // cout << "J_connect" << J_connect << endl;
    // cout << "J_fit" << J_fit << endl;

    return k_con_ * J_connect + k_fit_ * J_fit;
    // return 0.0;
}